#include "RK5.h"

__declspec(dllexport) SOLUTION RK5(pFUNC Func,SOLV_PARAM Param);
__declspec(dllexport) void Output(SOLUTION Solution);

//RKF45 solver core
__declspec(dllexport) SOLUTION RK5(pFUNC Func,SOLV_PARAM Param){
	SOLUTION Solution;
	Solution.n = Param.n;
	Solution.PointCount = 0;
	Solution.Curve = NULL;
	
	double t,h,h0;
	double *x,*xMid,*xAccu;
	double *k1,*k2,*k3,*k4,*k5,*k6;
	t = Param.a;
	h = Param.h;
	h0 = Param.h;
	x = (double *)malloc(Param.n*sizeof(double));
	xMid = (double *)malloc(Param.n*sizeof(double));
	xAccu = (double *)malloc(Param.n*sizeof(double));
	
	k1 = (double *)malloc(Param.n*sizeof(double));
	k2 = (double *)malloc(Param.n*sizeof(double));
	k3 = (double *)malloc(Param.n*sizeof(double));
	k4 = (double *)malloc(Param.n*sizeof(double));
	k5 = (double *)malloc(Param.n*sizeof(double));
	k6 = (double *)malloc(Param.n*sizeof(double));
	
	ArrayCopy(x,Param.x0,Param.n);	//Initial value
	
	int i;
	double OutFlag=0;
	int stepFlag = 0;
	while (t<Param.b)
	{
		//The end of the interval
		if ((Param.b-t)<h){
			h = Param.b-t+ERR_INF;
			OutFlag = 1; 
		}
		//k1
		for (i=0;i<Param.n;i++){
			k1[i] = h*(Func[i])(t,x);
			xMid[i] = x[i]+(1/4.0)*k1[i];
		}
		//k2
		for (i=0;i<Param.n;i++)
			k2[i] = h*(Func[i])(t+(1/4.0)*h,xMid);
		for (i=0;i<Param.n;i++)
			xMid[i] = x[i]+(3/32.0)*k1[i]+(9/32.0)*k2[i];
		//k3
		for (i=0;i<Param.n;i++)
			k3[i] = h*(Func[i])(t+(3/8.0)*h,xMid);
		for (i=0;i<Param.n;i++)
			xMid[i] = x[i]+(1932/2197.0)*k1[i]-(7200/2197.0)*k2[i]
						+(7296/2197.0)*k3[i];
		//k4
		for (i=0;i<Param.n;i++)
			k4[i] = h*(Func[i])(t+(12/13.0)*h,xMid);
		for (i=0;i<Param.n;i++)
			xMid[i] = x[i]+(439/216.0)*k1[i]-8.0*k2[i]+(3680/513.0)*k3[i]
					-(845/4104.0)*k4[i];
		//k5
		for (i=0;i<Param.n;i++)
			k5[i] = h*(Func[i])(t+h,xMid);
		//k6 & Two integral methods
		for (i=0;i<Param.n;i++){
			xMid[i] = x[i]-(8/27.0)*k1[i]+2*k2[i]-(3544/2565.0)*k3[i]
					+(1859/4104.0)*k4[i]-(11/40.0)*k5[i];
			k6[i] = h*(Func[i])(t+(1/2.0)*h,xMid);
			//RK5
			xAccu[i] = x[i]  +(16/135.0)*k1[i]  +(6656/12825.0)*k3[i]
						+(28651/56430.0)*k4[i]-(9/50.0)*k5[i]
						+(2/55.0)*k6[i];
		}	
		Solution.PointCount++;
			
		//New point creation
		Solution.Curve = (pPOINT)realloc(
		Solution.Curve,
		Solution.PointCount*sizeof(FPOINT));
			
		//Assign value to the new point
		t += h;
		(Solution.Curve[Solution.PointCount-1]).t = t;
		(Solution.Curve[Solution.PointCount-1]).x
			= (double *)malloc(Param.n*sizeof(double));
		ArrayCopy((Solution.Curve[Solution.PointCount-1]).x,xAccu,Param.n);
			
		//New integral point
		ArrayCopy(x,xAccu,Param.n);
		
		if (OutFlag==1)	break;
		
	}
	
	free(x);
	free(xMid);
	free(xAccu);
	free(k1);
	free(k2);
	free(k3);
	free(k4);
	free(k5);
	free(k6);
	
	return Solution;
}
//Output solution
__declspec(dllexport) void Output(SOLUTION Solution){
	int i,j;
	
	FILE *FileOut;
	FileOut = fopen("out.csv","w");
	for (i=0;i<Solution.PointCount;i++){
		fprintf(FileOut,"%f,",Solution.Curve[i].t);
		for (j=0;j<Solution.n;j++){
			fprintf(FileOut,"%f,",(Solution.Curve[i].x)[j]);
		}
		fputc('\n',FileOut);
	}
	
	fclose(FileOut);
}
//Copy array from double* to double*
void ArrayCopy(double *dest,double *src,int n){
	int i;
	for (i=0;i<n;i++)	dest[i] = src[i];
}
